map#!/usr/bin/env python2

import rospy
from cv_bridge import CvBridge
from cv_bridge.boost.cv_bridge_boost import getCvType
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import cv2
import uuid, datetime, time
from nav_msgs.msg import Path,Odometry
from std_msgs.msg import Int64,Bool
import os
import sys
import shutil
import numpy as np


dis_check_time_=0
lst_loc_=np.zeros((2))
_=False
stop_time_=0
count_=0
saving_=False

save_dir = ''
bridge = CvBridge()
last_save_time=0

def gps_odom_call(msg):
    # print("get new loc")
    global dis_check_time_, lst_loc_, _, count_
    loc=np.zeros((2))
    loc[0]=msg.pose.pose.position.x
    loc[1]=msg.pose.pose.position.y
    # loc = np.around(loc, 1)
    if np.all(lst_loc_==0):
        lst_loc_ = loc
        return

    if (msg.header.stamp.to_sec()-dis_check_time_)>3:
        dis_check_time_=msg.header.stamp.to_sec()
        check_stop(loc)
        
def check_stop(loc):
    global dis_check_time_, lst_loc_, _, stop_time_, count_, saving_
    is_stop=False
    # debug code
    # print("count", count_)
    # if (count_>10 and count_<20) or (count_>30 and count_<40) or (count_>50):
    #     loc = lst_loc_
    # count_+=1

    diff = loc - lst_loc_
    dis = np.linalg.norm(diff)

    if dis < 0.5:
        if (not is_stop):
            stop_time_=rospy.Time.now().to_sec()
        is_stop=True

    print("now car stop", is_stop)

def stereo_callback(data):
    global bridge,front_mid_cnt,time_step,last_save_time,save_dir
    cur_date = time.strftime("%Y%m%d")
    second = float(data.header.stamp.secs)
    second_fraction = float(data.header.stamp.nsecs)/1e9
    timestamp = second + second_fraction
    save_name = second

    if (second_fraction >= 0 and second_fraction < 0.5):
        save_name = second + 0.25
    else:
        save_name = second + 0.75
        
    time_diff = abs(timestamp-save_name)
    if time_diff < abs(last_save_time-save_name) and time_diff < 0.05:  
	    cv_image = bridge.imgmsg_to_cv2(data, "bgr8")
	    save_path = os.path.join(save_dir, cur_date)
	    if not os.path.exists(save_path):
	        os.makedirs(save_path)
	        
	    im_name=os.path.join(save_path, str(int(save_name*1e6)) + ".png")
	    print(im_name)
	    cv2.imwrite(im_name, cv_image)
	    last_save_time = timestamp
	    # print(last_save_time)
	
if __name__ == "__main__":
    
    save_dir = "/home/westwell/welldriver/data/sync_stereo/"
    if not os.path.exists(save_dir):
        os.makedirs(save_dir)

    rospy.init_node("save_sync_stereo_image", anonymous=True)
    rospy.Subscriber("/stereo_image", Image, stereo_callback)
    rospy.spin()

